L=2.065; %轴距
curp=[7 10 0]; %初始位置
fr=20; % 控制频率（Hz）
max_vel=3; % 最高速度（m/s）

% 控制器参数
lambda=0.001;
gamma=1;
beta=2.2;
h=1.5;
epsilon=0.9;

% 参考轨迹
theta_s=0:0.01:4*pi;
x=(15-6.25*cos(theta_s)).*(theta_s<pi)+(30.25-9*cos(theta_s-pi)).*(theta_s>=pi&theta_s<3*pi)+(15-6.25*cos(theta_s-2*pi)).*(theta_s>=3*pi);
y=(10-6.25*sin(theta_s)).*(theta_s<pi)+(10+9*sin(theta_s-pi)).*(theta_s>=pi&theta_s<3*pi)+(10-6.25*sin(theta_s-2*pi)).*(theta_s>=3*pi);
figure("Color","w");
plot(x,y,"LineWidth",2,"LineStyle","--","Color",[0.8,0.8,0.8]);
xlabel("E/m");
ylabel("N/m");
title("Trajectory tracking");
xlim([5,45]);
grid on;
grid minor;
axis equal;
hold on;

theta_r = 0;
vs=0;
v=0;
tan_delta=0;
count=0;
e_vector=[];
theta_vector=[];
alpha_vector=[];
v_vector=[];
vd_vector=[];
x_ref=[];
y_ref=[];
x_real=[];
y_real=[];
while (theta_r<=4*pi)&&(count<5000)
    % 定位
    x_last=curp(1);
    y_last=curp(2);
    curp(1)=curp(1)+v*cos(curp(3))/fr;
    curp(2)=curp(2)+v*sin(curp(3))/fr;
    curp(3)=curp(3)+v*tan_delta/L/fr;
    plot([x_last,curp(1)],[y_last,curp(2)],"Color",[0,0.45,0.74],"LineWidth",2);
    hold on;

    % 规划
    if theta_r<pi
        dtheta=vs/6.25/fr;
    elseif theta_r<3*pi
        dtheta=vs/9/fr;
    else
        dtheta=vs/6.25/fr;
    end
    theta_r=theta_r+dtheta;
    [x_d,y_d,phi_d,vel_d]=calPos(theta_r);
%     plot(x_d,y_d,".","Color","red","MarkerSize",8);
%     hold on;

    % 控制
    e=sqrt((x_d-curp(1))^2+(y_d-curp(2))^2);
    v1=[cos(phi_d) sin(phi_d)];
    v2=[x_d-curp(1) y_d-curp(2)];
    v2=v2/norm(v2);
    theta=atan2(v1(1)*v2(2)-v2(1)*v1(2),v1*v2');
    v1=[cos(curp(3)) sin(curp(3))];
    alpha=atan2(v1(1)*v2(2)-v2(1)*v1(2),v1*v2');
    V=lambda*e^2+alpha^2+h*theta^2;
    if V>epsilon
        % 停车问题
        vs=0;
    else
        % 轨迹跟踪
        vs=vel_d*(1-V/epsilon);
    end
    % 控制输出（车速及转角）
    v=gamma*e;
    if v>max_vel
        v=max_vel;
    end
    if e==0
        tmp=L*(sin(alpha)+h*theta*sin(alpha)/alpha+beta*alpha);
        if tmp>0
            tan_delta=max_tan_delta;
        elseif tmp<0
            tan_delta=-max_tan_delta;
        else
            tan_delta=0;
        end
    else
        if alpha==0
            tan_delta=L*(sin(alpha)/e+h*theta/e+beta*alpha/e);
        else
            tan_delta=L*(sin(alpha)/e+h*theta/e*sin(alpha)/alpha+beta*alpha/e);
        end
    end
    e_vector=[e_vector,e];
    theta_vector=[theta_vector,theta];
    alpha_vector=[alpha_vector,alpha];
    v_vector=[v_vector,v];
    vd_vector=[vd_vector,vel_d];
    x_real=[x_real,curp(1)];
    y_real=[y_real,curp(2)];
    x_ref=[x_ref,x_d];
    y_ref=[y_ref,y_d];
    count=count+1;
end
legend(["Reference","Actual"]);

figure("Color","w");
plot((0:1:(count-1))/fr,e_vector,"LineWidth",2);
hold on;
plot((0:1:(count-1))/fr,theta_vector,"LineWidth",2,"LineStyle","--");
hold on;
plot((0:1:(count-1))/fr,alpha_vector,"LineWidth",2,"LineStyle",":");
hold on;
plot((0:1:(count-1))/fr,vd_vector-v_vector,"LineWidth",2,"LineStyle","-.");
title("Error");
grid on;
grid minor;
xlabel("Time/s");
ylabel("Amplitude");
legend(["e(m)","\theta(rad)","\alpha(rad)","v_e(m/s)"],"Location","southeast");

figure("Color","w");
plot((0:1:(count-1))/fr,v_vector,"LineWidth",2);
hold on;
plot((0:1:(count-1))/fr,vd_vector,"LineWidth",2,"LineStyle","--");
title("Velocity");
grid on;
grid minor;
xlabel("Time/s");
ylabel("Velocity");
legend(["v(m/s)","v_r_e_f(m/s)"],"Location","southeast");

figure("Color","w");
plot(15,10);
xlabel("E/m");
ylabel("N/m");
title("Trajectory tracking");
xlim([5,45]);
grid on;
grid minor;
axis equal;
hold on;
mycomet(x_real,y_real,x_ref,y_ref);

function [x,y,phi,vel]=calPos(theta)
    vel=1.5+0.5*sin(2*theta);
%     vel=2;
    if theta<pi
        x=15-6.25*cos(theta);
        y=10-6.25*sin(theta);
        phi=-pi/2+theta;
    elseif theta<3*pi
        x=30.25-9*cos(theta-pi);
        y=10+9*sin(theta-pi);
        if theta <2.5*pi
            phi=0.5*pi-(theta-pi);
        else
            phi=pi-(theta-2.5*pi);
        end
    else
        x=15-6.25*cos(theta-2*pi);
        y=10-6.25*sin(theta-2*pi);
        if theta<3.5*pi
            phi=0.5*pi+(theta-3*pi);
        else
            phi=-pi+(theta-3.5*pi);
        end
    end
end